﻿using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Diagnostics;

using Debug = UnityEngine.Debug;

public class Node3D : System.IComparable
{
    public static bool operator ==(Node3D a, Node3D b)
    {
        if ((object)a == null)
            return (object)b == null;
        if ((object)b == null)
            return (object)a == null;

        return a.position == b.position;
    }
    public static bool operator !=(Node3D a, Node3D b)
    {
        return !(a == b);
    }
    public static bool operator <(Node3D a, Node3D b)
    {
        return a.f < b.f;
    }
    public static bool operator >(Node3D a, Node3D b)
    {
        return a.f > b.f;
    }
    public static bool operator <=(Node3D a, Node3D b)
    {
        return a.f <= b.f;
    }
    public static bool operator >=(Node3D a, Node3D b)
    {
        return a.f >= b.f;
    }

    public int CompareTo(object obj)
    {
        Node3D node = obj as Node3D;

        if (f < node.f)
            return -1;

        if (f > node.f)
            return 1;

        return 0;
    }

    public override bool Equals(object obj)
    {
        return position.Equals((obj as Node3D).position);
    }
    public bool Equals(Node3D node)
    {
        return position.Equals(node.position);
    }
    public override int GetHashCode()
    {
        return position.GetHashCode();
    }
    public override string ToString()
    {
        return string.Format("Node: pos({0}), isWalkable({1}), g: {2}, h: {3}, f:{4}", position, isWalkable, g, h, f);
    }

    public Point3 position { get; private set; }

    public bool isWalkable { get; set; }
    public int g;
    public float f { get { return g + h; } }
    public float h;

    public bool InOpenlist = false;
    public bool InClosedlist = false;

    public Node3D parent;

    /// <summary>
    /// Constructs the start node
    /// </summary>
    /// <param name="startPos">The position of the node</param>
    public Node3D(Point3 startPos)
    {
        position = startPos;
        h = g = 0;
        isWalkable = true;
    }
    /// <summary>
    /// Constructs a common node on the grid
    /// </summary>
    /// <param name="pos">The position of the node</param>
    /// <param name="EndNode">The start node of the path</param>

    public Node3D(Point3 pos, Point3 TargetPos, bool isWalkable = true)
    {
        position = pos;
        g = int.MaxValue;
        h = (pos - TargetPos).magnitude * 125;// (Mathf.Abs(pos.x - TargetPos.x) + Mathf.Abs(pos.y - TargetPos.y) + Mathf.Abs(pos.z - TargetPos.z)) * 100;
        this.isWalkable = isWalkable;
    }
}

public static class Pathfinding
{
    private const int SEARCH_RANGE = 2048;
    private const int SEARCH_RANGE_HALF = SEARCH_RANGE / 2;
    private const int SEARCH_RANGE_PLUS1 = SEARCH_RANGE + 1;
    private const int SEARCH_RANGE_PLUS1_SQUARED = SEARCH_RANGE_PLUS1 * SEARCH_RANGE_PLUS1;
    private const int SEARCH_HEIGHT = 64; // Used for 3D Pathfinding
    private const int SEARCH_HEIGHT_PLUS1 = SEARCH_HEIGHT + 1;
    private const int SEARCH_HEIGHT_HALF = SEARCH_HEIGHT / 2;

    private const int P_COSTS_1D = 100; // sqrt(1) * 100
    private const int P_COSTS_2D = 141; // sqrt(2) * 100
    private const int P_COSTS_3D = 173; // sqrt(3) * 100

    private static Dictionary<Point3, Node3D> NodeArray_3D;
    private static Node3D Start_3D;
    private static Node3D End_3D;
    private static PriorityQueue<float, Node3D> OpenList_3D;
    private static List<Node3D> ClosedList_3D;


    static Pathfinding()
    {
        NodeArray_3D = new Dictionary<Point3, Node3D>();
        OpenList_3D = new PriorityQueue<float, Node3D>();
        ClosedList_3D = new List<Node3D>();
    }

    public static List<Point3> FindPath(Point3 begin, Point3 target, VoxelTerrain voxelTerrain)
    {
        #region Exception Handling
        // Out of Search range
        if (Mathf.Abs(begin.x - target.x) > SEARCH_RANGE_HALF || Mathf.Abs(begin.z - target.z) > SEARCH_RANGE_HALF || Mathf.Abs(begin.y - target.y) > SEARCH_HEIGHT_HALF)
        {
            Debug.LogWarning("Target point to far away from start point. No path can be calculated!");

            int deltaX = Mathf.Clamp(target.x - begin.x, -SEARCH_RANGE_HALF, SEARCH_RANGE_HALF);
            int deltaZ = Mathf.Clamp(target.z - begin.z, -SEARCH_RANGE_HALF, SEARCH_RANGE_HALF);

            Point3 newTarget = new Point3(begin.x + deltaX, voxelTerrain.HeightmapLayer(begin.x + deltaX, begin.z + deltaZ), begin.z + deltaZ);

            return Pathfinding.FindPath(begin, newTarget, voxelTerrain).Union(Pathfinding.FindPath(newTarget, target, voxelTerrain)).ToList();
        }
        // Start == Target
        if (begin == target)
        {
            Debug.LogWarning("Target point is the same as begin point. No path is needed to be calculated!");
            List<Point3> result = new List<Point3>();
            result.Add(begin);
            return result;
        }
        #endregion

        NodeArray_3D.Clear();
        OpenList_3D.Clear();
        ClosedList_3D.Clear();

        Start_3D = new Node3D(begin);
        End_3D = new Node3D(target, target);

        // Fill the NodeList
        NodeArray_3D.Add(begin, Start_3D);
        NodeArray_3D.Add(target, End_3D);

        OpenList_3D.Insert(Start_3D.f, Start_3D);
        Start_3D.InOpenlist = true;

        Node3D currentNode = null;

        while (OpenList_3D.Count > 0)
        {
            currentNode = OpenList_3D.RemoveMin().Value;
            currentNode.InOpenlist = false;

            //Debug.Log(currentNode);

            if (currentNode == End_3D)
            {
                return Path_3D();
            }

            ClosedList_3D.Add(currentNode);
            currentNode.InClosedlist = true;

            //GameObject go = GameObject.CreatePrimitive(PrimitiveType.Cube);
            //go.transform.position = currentNode.position;
            ExpandNode_3D(currentNode, voxelTerrain);
        }

        return null;
    }

    private static void ExpandNode_3D(Node3D node, VoxelTerrain voxelTerrain)
    {
        Point3 index = node.position;

        Node3D currentNode;

        Point3[] indices = new Point3[] { // P_Cost_1D
                                    index + new Point3(1, 0, 0), // Right
                                    index + new Point3(-1, 0, 0), // Left
                                    index + new Point3(0, 0, 1), // Front 
                                    index + new Point3(0, 0, -1), // Back 

                                    // P_Cost_2D
                                    index + new Point3(1, 0, 1)  , // Front Right
                                    index + new Point3(-1, 0, 1) ,  // Front Left
                                    index + new Point3(1, 0, -1) , // Back Right
                                    index + new Point3(-1, 0, -1), // Back Left

                                    index + new Point3(1, 1, 0), // Top Right 
                                    index + new Point3(-1, 1, 0), // Top Left
                                    index + new Point3(0, 1, 1),  // Top Front
                                    index + new Point3(0, 1, -1),  // Top Back

                                    index + new Point3(1, -1, -1), // index + 1 - SEARCH_RANGE_PLUS1_SQUARED,                     // Bottom Right 
                                    index + new Point3(-1, -1, -1),// index - 1 - SEARCH_RANGE_PLUS1_SQUARED,                    // Bottom Left
                                    index + new Point3(0, -1, 1),  // index + SEARCH_RANGE_PLUS1 - SEARCH_RANGE_PLUS1_SQUARED,     // Bottom Front
                                    index + new Point3(0, -1, -1), // index - SEARCH_RANGE_PLUS1 - SEARCH_RANGE_PLUS1_SQUARED,    // Bottom Back

                                    // P_Cost_3D
                                    index + new Point3(1, 1, 1),   // index + 1 + SEARCH_RANGE_PLUS1 + SEARCH_RANGE_PLUS1_SQUARED,  // Top Front Right
                                    index + new Point3(-1, 1, 1),  // index - 1 + SEARCH_RANGE_PLUS1 + SEARCH_RANGE_PLUS1_SQUARED,  // Top Front Left
                                    index + new Point3(1, 1, -1),  // index + 1 - SEARCH_RANGE_PLUS1 + SEARCH_RANGE_PLUS1_SQUARED, // Top Back Right
                                    index + new Point3(-1, 1,- 1), // index - 1 - SEARCH_RANGE_PLUS1 + SEARCH_RANGE_PLUS1_SQUARED, // Top Back Left

                                    index + new Point3(1, -1, 1),  //index + 1 + SEARCH_RANGE_PLUS1 - SEARCH_RANGE_PLUS1_SQUARED, // Bottom Front Right
                                    index + new Point3(-1, -1, 1), //index - 1 + SEARCH_RANGE_PLUS1 - SEARCH_RANGE_PLUS1_SQUARED, // Bottom Front Left
                                    index + new Point3(1, -1, -1), //index + 1 - SEARCH_RANGE_PLUS1 - SEARCH_RANGE_PLUS1_SQUARED, // Bottom Back Right
                                    index + new Point3(-1, -1, -1) //index - 1 - SEARCH_RANGE_PLUS1 - SEARCH_RANGE_PLUS1_SQUARED  // Bottom Back Left
        };

        for (int i = 0; i < indices.Length; i++)
        {
            // Check if node index is inside the array (or in search range)
            if (indices[i].x >= Start_3D.position.x - SEARCH_RANGE_HALF && indices[i].x < Start_3D.position.x + SEARCH_RANGE_HALF &&
                indices[i].y >= Start_3D.position.y - SEARCH_HEIGHT_HALF && indices[i].y < Start_3D.position.y + SEARCH_HEIGHT_HALF &&
                indices[i].z >= Start_3D.position.z - SEARCH_RANGE_HALF && indices[i].z < Start_3D.position.z + SEARCH_RANGE_HALF)
            {
                if (!NodeArray_3D.ContainsKey(indices[i]))
                {
                    NodeArray_3D.Add(indices[i], new Node3D(indices[i], End_3D.position, voxelTerrain.GetBlockAt(indices[i] + new Point3(0, -1, 0)) != Block.Air &&
                                                                                         voxelTerrain.GetBlockAt(indices[i] + new Point3(0, 0, 0)) == Block.Air &&
                                                                                         voxelTerrain.GetBlockAt(indices[i] + new Point3(0, 1, 0)) == Block.Air));
                }

                currentNode = NodeArray_3D[indices[i]];
                if (currentNode == End_3D)
                    currentNode = End_3D;

                if (!currentNode.InClosedlist && currentNode.isWalkable)
                {
                    if (!currentNode.InOpenlist)
                    {
                        currentNode.parent = node;
                        currentNode.g = node.g + ((i < 4) ? P_COSTS_1D : (i < 16) ? P_COSTS_2D : P_COSTS_3D);

                        OpenList_3D.Insert(currentNode.f, currentNode);
                        currentNode.InOpenlist = true;
                    }
                    else if (node.g + ((i < 4) ? P_COSTS_1D : ((i < 16) ? P_COSTS_2D : P_COSTS_3D)) < currentNode.g)
                    {
                        currentNode.g = node.g + ((i < 4) ? P_COSTS_1D : ((i < 16) ? P_COSTS_2D : P_COSTS_3D));
                        currentNode.parent = node;
                        OpenList_3D.UpdateKey(currentNode, currentNode.f);
                    }
                }
            }
        }
    }

    private static List<Point3> Path_3D()
    {
        //Debug.Log(NodeArray_3D.Keys.Count);
        Node3D currentNode = End_3D;

        List<Point3> PathFound = new List<Point3>();
        PathFound.Add(End_3D.position);
        //GameObject go = GameObject.CreatePrimitive(PrimitiveType.Cube);
        //go.transform.position = new Vector3(End.position.x, 0, End.position.y);

        while (currentNode != Start_3D)
        {
            currentNode = currentNode.parent;
            PathFound.Add(currentNode.position);

            //GameObject go2 = GameObject.CreatePrimitive(PrimitiveType.Cube);
            //go2.transform.position = new Vector3(currentNode.position.x, 0, currentNode.position.y);
        }

        PathFound.Reverse();

        return PathFound;
    }
}